#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
//#include <sensor_msgs/Image.h>
#include <vector>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int main(){
    rosbag::Bag bag;
    bag.open("/home/st/ubuntu_data/dataset/gazebo/0122data_ok/run_trace_train.bag", rosbag::bagmode::Read);

    std::vector<std::string> topics;
    topics.push_back(std::string("/robot1/Ruby80"));
    topics.push_back(std::string("/robot1/ground_truth/feedback"));
    topics.push_back(std::string("/husky_black/ground_truth/feedback"));
    topics.push_back(std::string("/husky_green/ground_truth/feedback"));
    topics.push_back(std::string("/husky_white/ground_truth/feedback"));

    rosbag::View view(bag, rosbag::TopicQuery(topics));

    foreach(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::PointCloud2::ConstPtr pointcloud = m.instantiate<sensor_msgs::PointCloud2>();
        if (pointcloud != NULL)
            std::cout <<"pointcloud" <<pointcloud->header.stamp << std::endl;
        nav_msgs::Odometry::ConstPtr master_traj_gt = m.instantiate<nav_msgs::Odometry>();
        if (master_traj_gt != NULL)
            std::cout << master_traj_gt->header.stamp << std::endl;
        nav_msgs::Odometry::ConstPtr slave_black_traj_gt = m.instantiate<nav_msgs::Odometry>();
        if (master_traj_gt != NULL)
            std::cout << slave_black_traj_gt->header.stamp << std::endl;
        nav_msgs::Odometry::ConstPtr slave_green_traj_gt = m.instantiate<nav_msgs::Odometry>();
        if (master_traj_gt != NULL)
            std::cout << slave_green_traj_gt->header.stamp<< std::endl;
        nav_msgs::Odometry::ConstPtr slave_white_traj_gt = m.instantiate<nav_msgs::Odometry>();
        if (master_traj_gt != NULL)
            std::cout << slave_white_traj_gt->header.stamp<< std::endl;
    }
    bag.close();
}
